/******************************************************************************
**
** File Name:	rtos_task_ex.S
**
** Description:	RTOS extended task services.
**
** RTOS ver:	2.0
**
** Modules/Functions:
**
**			RTOS_TaskYield()
**			RTOS_TaskYieldISR()
**			RTOS_TaskGetId()			// not verified
**			RTOS_TaskGetPriority()		// not verified	
**			RTOS_TaskSetPriority()		// not verified
**			RTOS_TaskTerminate()		// not verified
**			RTOS_TaskExit()				// not verified
**
** Notes:	none
** 		
*******************************************************************************
** History:
**
**	07/26/2012 - AK - RTOS API is downgraded to comply with earliest rtos v2
**  08/08/2012 - AK - RTOS source code is checked for byte-by-byte complience 
**					with MPP binary (Dec 8, 2011).
**
******************************************************************************/
#include        "rtos.inc"

/******************************************************************************
** MODULE::RTOS_TASK_YIELD
******************************************************************************/
        _MODULE(rtos_task_ex)

        _EXTERN(_EnterISR)
        _EXTERN(_LeaveISR)
        _EXTERN(RtosKernelData)
        _EXTERN(_EnqueueObj)
        _EXTERN(_RemoveObj)
	
        _CODESECTION

/******************************************************************************
;; Prototype:	void RTOS_TaskYield(void)
;; Description:	Remove self from running task queue and do enqueue again.
;;				If other task with same priority are in the queue, then 
;;				will be attached behind them (cooperative scheduling).
;; Passed:		
;; Returns:		
;; Uses:		Z, p1l:p1h, p2l:p2h
;; Calls:		_EnqueueObj, _LeaveISR, _EnterISR
;; Assumes:		User mode
;; Notes: 		We remove "Running" which may or may not be the top of the 
;;				queue depending upon whatever other interrupt activity might 
;;				be going on.  If we just removed the top of the run queue, 
;;				it would work virutally all the time, but technically, it 
;;				could fail to reschedule the correct task. 
******************************************************************************/
        _FUNCTION(RTOS_TaskYield)

RTOS_TaskYield:

	_ENTER_CRITICAL
	
	_RTOS_PROLOG
	
	ldi	Zl, lo8(RtosKernelData+RunQueue)
	ldi	Zh, hi8(RtosKernelData+RunQueue)
	ldd	p2l, Z+Running+NextL
	ldd	p2h, Z+Running+NextH
	
	rcall	_RemoveObj					;; Can't fail, so don't bother checking
		
	mov	p1l, p2l
	mov	p1h, p2h
	
	rcall	_EnqueueObj					;; requeue ourselves
	
	_RTOS_EPILOG

    _ENDFUNC	;; _FUNCTION(RTOS_TaskYield)
		
/******************************************************************************
;; Prototype:	void RTOS_TaskYieldISR(void)
;; Description:	The version of the RTOS_TaskYield() to call from kernel
;;				mode (for example from interrupt). Will reschedule the top of 
;;				the queue, which could be not actually the current running 
;;				task (see description of RTOS_TaskYield above).
;; Passed:		None
;; Returns:		None
;; Uses:		Z, p1l:p1h
;; Calls:		_EnqueueObj
;; Assumes:		Kernel mode
;; Notes:		Probably is better to add parameter and modify it to 
;;				to work for specified TaskID.
******************************************************************************/
    _FUNCTION(RTOS_TaskYieldISR)

RTOS_TaskYieldISR:

	_ENTER_CRITICAL

	lds	Zl, RtosKernelData+RunQueue+NextL	;; Grab the top of the run queue
	lds	Zh, RtosKernelData+RunQueue+NextH
	adiw	Zl, 0
	
	brne	rt_tyi00
	
	ret										;; Exit if empty
									
rt_tyi00:

	ldd	p1l, Z+NextL						;; Point to the next
	ldd	p1h, Z+NextH
	sts	RunQueue+NextL, p1l
	sts	RunQueue+NextH, p1h
	
	rjmp	_EnqueueObj

	_LEAVE_CRITICAL

    _ENDFUNC	;; _FUNCTION(RTOS_TaskYieldISR)

/******************************************************************************
** Prototype:	void RTOS_TaskGetId(void)
** Description:	
** Passed:		
** Returns:		
** Uses:		
** Calls:		
** Assumes:		
** Notes:		
******************************************************************************/
    _FUNCTION(RTOS_TaskGetId)

RTOS_TaskGetId:
    lds     r1h, RtosKernelData+RunQueue+NextH
    lds     r1l, RtosKernelData+RunQueue+NextL
	
    ret

    _ENDFUNC	;; _FUNCTION(RTOS_TaskGetId)	

/******************************************************************************
** Prototype:	unsigned char RTOS_TaskGetPriority(RTOS_TCB_t * )
** Description:	Get the priority of the Task specified by TaskID
** Passed:		p1l:p1h = pointer to task control block
** Returns:		Priority of the specified task
** Uses:		None
** Calls:		None
** Assumes:		None
** Notes:		None
******************************************************************************/
    _FUNCTION(RTOS_TaskGetPriority)

RTOS_TaskGetPriority:

    mov     Zl, p1l							;; Zl:Zh = PID
    mov     Zh, p1h

;;    lds     Zh, RtosKernelData+RunQueue+NextH
;;    lds     Zl, RtosKernelData+RunQueue+NextL

    ldd     r1l, Z+PidPriority
	
    ret
	
    _ENDFUNC	;; _FUNCTION(RTOS_TaskGetPriority)
		

/******************************************************************************
** Prototype:	U8 RTOS_TaskSetPriority(RTOS_TCB_t *, U8 )
** Description:	Set the priority of the task specified by TaskID
** Passed:		RtosTaskID * 
**				Priority
** Returns:		Priority
** Uses:		
** Calls:		
** Assumes:		Top of RunQueue = Current running task
** Notes:		
******************************************************************************/
    _FUNCTION(RTOS_TaskSetPriority)

RTOS_TaskSetPriority:

    mov     Zl, p1l
    mov     Zh, p1h
    ldd     r1l, Z+PidPriority
    std     Z+PidPriority, p2l
    ret

    _ENDFUNC	;; _FUNCTION(RTOS_TaskSetPriority)

/******************************************************************************
** Prototype:	void RTOS_TaskExit(RTOS_TCB_t * )
** Description:	Exit the specified task.
** Passed:		p1l:p1h = pointer to TaskID to terminate
** Returns:		None
** Uses:		Z, X, p1l:p1h, p2l:p2h
** Calls:		_EnterISR, RTOS_TerminateTask
** Assumes:		None
** Notes:		None
******************************************************************************/
    _FUNCTION(RTOS_TaskExit)					
	
RTOS_TaskExit:                   				;; Entry point for killing self
												;; Outdated service. To be removed.

    call 	_EnterISR             				;; Entire state of killing task is on stack
	
    lds     p1h, RtosKernelData+Running+NextH
    lds     p1l, RtosKernelData+Running+NextL
	
    rjmp    rt_tt00
	
    _ENDFUNC

/******************************************************************************
** Prototype:	void RTOS_TaskTerminate(RTOS_TCB_t * )
** Description:	Terminate the task specified. Sets the "Idle" bit, so the 
**				task cannot be enqueued again. Attempting to remove from the 
**				running task queue (will fail if task is blocked).
** Passed:		R25:R24 = pointer to TaskID to terminate
** Returns:		None
** Uses:		Z, X, p1l:p1h, p2l:p2h
** Calls:		_EnterISR, _RemoveObj, _LeaveISR
** Assumes:		None
** Notes:		None
******************************************************************************/
   _FUNCTION(RTOS_TaskTerminate)
	
RTOS_TaskTerminate:	               				;; Entry point for killing other tasks.

	call	_EnterISR
	
rt_tt00:

    mov     Zh, p1h
    mov     Zl, p1l
    ldi     Xl, BV(IdleBit) ; Mark task dead
    std     Z+PidState, Xl
    mov     p2h, p1h
    mov     p2l, p1l
    ldi     Zh, hi8(RtosKernelData+RunQueue)
    ldi     Zl, lo8(RtosKernelData+RunQueue)
	
    ;; -=AAK022=- cli
		
    rcall   _RemoveObj           				;; Attempt to remove from run queue
	
    rjmp    _LeaveISR							;; return to task that is on top of the queue
												;; or to _IdleTask if no more running tasks 
												
    _ENDFUNC	;; _FUNCTION(RTOS_TaskTerminate)
	
		_END 	;; _MODULE(rtos_task_ex)

